
#include "MatVec.h"
#include <GL/glut.h>



// A very simple plane class. The plane is defined as a point and normal.
// Many routines depend on non-object oriented math routines pulled from the
// UNC PQP distance package. See MatVec.h for definitions.
class plane
{
public:
    double N[3];
    double p[3];

    // Draw the plane. A plane has infinite extend but we will draw it bounded
    void draw(double width = 1.0)
	{

	    // The hard thing is to create a full local coordinate system so
	    // we can draw the corners. First, pick a vector to cross with, then
	    // make the full 3 ortho vectors.
	    double vec1[3], vec2[3];
		vec1[0] = vec1[1] = vec1[2] = 0.0;
	    if ( N[2] > 0.8 )
		vec1[1] = 1.0;
	    else
		vec1[2] = 1.0;
	    
	    VcrossV( vec2, vec1, N );
	    Vnormalize( vec2 );
	    VcrossV( vec1, vec2, N );

	    double A[3], B[3], C[3], D[3], tmp[3];

	    // Find the corners A,B,C,D
	    VpVxS(tmp, p, vec1, width );
	    VpVxS(A, tmp, vec2, width );
	    VmVxS(B, tmp, vec2, width );
	    VmVxS(tmp, p, vec1, width );
	    VpVxS(C, tmp, vec2, width );
	    VmVxS(D, tmp, vec2, width );

	    // Draw the filled plane. I actually draw the corners counter-clockwise
	    // from what they should be so that they disappear with back face culling on.
	    // This is so you always see the back side of the box, no matter how it is rotated.
	    glBegin(GL_QUADS);
	    glVertex3d( A[0], A[1], A[2] );
	    glVertex3d( C[0], C[1], C[2] );
	    glVertex3d( D[0], D[1], D[2] );
	    glVertex3d( B[0], B[1], B[2] );
	    glEnd();

	    // Draw the plane outlined as well. 
	    glLineWidth(5.0);
	    glDisable(GL_LIGHTING);
	    glBegin(GL_LINE_LOOP);
	    glVertex3d( A[0], A[1], A[2] );
	    glVertex3d( B[0], B[1], B[2] );
	    glVertex3d( D[0], D[1], D[2] );
	    glVertex3d( C[0], C[1], C[2] );
	    glEnd();
	    glEnable(GL_LIGHTING);
	    glLineWidth(1.0);

	}
};

// A simple sphere class
class sphere
{
public:
    double p[3]; // position
    double r;    // radius
    double v[3]; // velocity
    double mass; // mass
	double haptic_force[3]; //force applied by the haptics device

    // compute the signed distance between two spheres. When they are colliding
    // a negative distance is returned.
    double distSphereSphere( const sphere &s )
	{
	    return sqrt(VdistV2(p, s.p)) - r - s.r;
	}

    
    // compute the signed distance between a sphere and plane. The plane normal determines
    // which side is 'inside'. When they are colliding a negative distance is returned.
    double distSpherePlane( const plane &P)
	{
	    double vec[3];

	    VmV( vec, p, P.p );
	    return VdotV( vec, P.N ) - r;
	}

    // Draw the sphere using glutSolidSphere
    void draw()
	{
	    
	    glPushMatrix();
	    glTranslated(p[0],p[1],p[2]);
	    glutSolidSphere( r, 10, 10 );
	    glPopMatrix();
	}
    
};



